#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "mysystem.h"
#include "myhall.h"
#include "mymath.h"
#include "mypwm.h"
#include "my485.h"
#include "tmp112a.h"


extern uint8_t debugval;
extern modbus_TypeDef Modbus;    //定义结构体
uint16_t currentbuf[adc_max];
char usart3_TxBuffer[100];   // 串口发送缓存
char usart3_TxBuffer1[16];   // 串口发送缓存
uint8_t state_mark;
uint8_t view_count;
extern uint8_t DMA_TxDone;
extern bool dma_updated;
float Theta_Accumulate;
float actual_postion;
bool reverse_pid_flag=false;
uint32_t pid_count;
float bus_current;
uint8_t temp_motor_dir;
bool normal_stop_flag = true; // false denote the error_stop state
extern bool adc_data_ready;
extern bool commucation_flag;

//uint32_t accelerate_count=0;
int32_t accelerate_count=0;
bool accelerate_flag=false;
bool soft_trigger_flag=false;
float target_speed0;



#define setting_speed 8000.0f
#define motor_PolePair 7.0f
#define DMA_TIMEOUT_THRESHOLD 4000  // 50us * 4000 = 200ms

void UART_Print_MotorSpeed(void);
void UART_Print_ViewCount(void);
void Oscillate_Task(void);
void UART_Print_HallState(void);
void Position_Speed_PI_Task(void);

MotorState_t motor_state;

void TaskManager(void)
{
	
	if(Motor_Vars.motorStopFlAG == 1)
	{
		Motor_Stop_Task();
		Motor_Vars.motorStopFlAG = 0;
	}
	if(Motor_Vars.motorStartFlAG == 1)
	{
		Motor_Start_Task();
		TIM6->CNT = 0;
		Motor_Vars.motorStartFlAG = 0;
	}


//	if(adc_data_ready)
//	{
//		adc_data_ready = 0;
//		HAL_ADC_Start_DMA(&hadc1, (uint32_t *)My_adcData, adc_max);
//	}

	Oscillate_Task();

	uint8_t count = 0;

//	if((motor_state==START)&&count==1)
//	{
//		count++;
//	}
//	else
//	{
//		return;
//	}
//	if((motor_state==REVERSE_DECEL))
//	{
//		return;
//	}

	if(TimeVars.periodFLAG_50us)
	{
//		if(DMA_TxDone&&(motor_state==START))
//		{
//			DMA_TxDone=0;
//			UART_Print_MotorSpeed();
//		}
		if(DMA_TxDone)
		{
			DMA_TxDone=0;
			UART_Print_MotorSpeed();
		}
		TimeVars.periodFLAG_50us = 0;
	}


	if(TimeVars.periodFLAG_1ms)
	{
		MotorStateDetect();

		TimeVars.periodFLAG_1ms = 0;
	}

	if(TimeVars.periodFLAG_200ms)
	{
		TimeVars.periodFLAG_200ms = 0;
	}

	if(TimeVars.periodFLAG_10ms)
	{
		// hall decide the state of motor

//		if((Motor_Vars.Motor_spinFlag==0)&&((motor_state==FORWARD)||(motor_state==REVERSE_ACCEL)))
//		{
//			Motor_Start_Task();
//			Motor_Start_Task();
//		}
		TimeVars.periodFLAG_10ms = 0;
	}

	if(TimeVars.periodFLAG_500ms)
	{

		TimeVars.periodFLAG_500ms = 0;
	}
}

void Oscillate_Task(void)
{
	static uint8_t count = 0;
	static bool stop_first_in = true;
	static bool reverse_first_in = true;
	static bool reverseDecel_first_in = true;
	static bool start_first_in = true;
	static uint32_t start = 0;

	if(count==0)
	{
		motor_state = START;
		count++;
	}

	switch (motor_state)
	{
	    case START:
	    	if(start_first_in)
	    	{
		    	// setting speed
		    	Motor_Vars.Motor_AimSpeed = setting_speed;

				// update or initialize varies
		    	stop_first_in = true;
		    	reverse_first_in = true;
		    	reverseDecel_first_in = true;

				// speed_control and position accumulate
				Theta_Accumulate = 144000.0f;
				reverse_pid_flag = false;
				start_first_in = false;

		    	pretarget_finish_flag = true;

		    	// 1. start hall 2. enable tim1 com it 3.generate software com
		    	actual_postion = 0;
		    	accelerate_count = 0;
	    		Motor_Start_Task();


	    	}
	    	HAL_Delay(1);
	    	Motor_Vars.motorStateFLAG = MOTOR_RUN;
	    	motor_state = FORWARD;
	        break;

	    case FORWARD:
	    	if(fabs(actual_postion-Theta_Accumulate)<120.0f)
	    	{
	    		motor_state = DECELERATE;
	    	}
	        break;

	    case DECELERATE:
	    	if(stop_first_in)
	    	{
	   		    Motor_Vars.motorStateFLAG = MOTOR_IDLE;
	   		    Motor_Vars.Motor_ActuralDuty = 200;
	   		    pretarget_finish_flag = false;
	    	}


			if(Motor_Vars.Motor_spinFlag == 0)
			{
				// clear the integral of pid0
				Position_PI.I = 0.0f;
	    		pretarget_finish_flag = true;
	    		actual_postion = 0;
	    		accelerate_count = 0;
//				Position_PI.I = 0.0f;
				motor_state = REVERSE_ACCEL;
			}

	        break;

	    case REVERSE_ACCEL:
	    	if(reverse_first_in)
	    	{
	    		Motor_Vars.Motor_DIR^=0x01;

		    	// 1. start hall 2. enable tim1 com it 3.generate software com
	    		Motor_Start_Task();
	    		HAL_Delay(1);
	    		reverse_first_in = false;

	    		// setting reverse_pid_flag and initialize actual position
	    		reverse_pid_flag = true;

	    		  Motor_Vars.motorStateFLAG = MOTOR_RUN;
	    	}


	    	if(fabs(actual_postion-Theta_Accumulate)<120.0f)
	    	{
	    		motor_state = REVERSE_DECEL;
	    	}
	        break;

	    case REVERSE_DECEL:
	    	if(reverseDecel_first_in)
	    	{
	   		    Motor_Vars.motorStateFLAG = MOTOR_IDLE;
	   		    Motor_Vars.Motor_ActuralDuty = 200;
	   		    pretarget_finish_flag = false;
	    		reverseDecel_first_in = false;
	    	}

			if(Motor_Vars.Motor_spinFlag == 0)
			{
				Motor_Vars.Motor_DIR^=0x01;

				HAL_TIM_PWM_Stop(&htim1,TIM_CHANNEL_1);
				HAL_TIM_PWM_Stop(&htim1,TIM_CHANNEL_2);
				HAL_TIM_PWM_Stop(&htim1,TIM_CHANNEL_3);

				Position1_PI.I = 0.0f;
				motor_state = START;
				start_first_in = true;
				pretarget_finish_flag = true;
				break;
			}
	        break;
	}

}


void UART_Print_MotorSpeed(void)
{
//	sprintf(usart3_TxBuffer, "aim %ld, act: %.2f, state: %.2f, duty: %.2f, theta: %.2f, pid: %.2f\n",
//			(uint32_t)accelerate_count,(float)Motor_Vars.Motor_ActuralSpeed,(float)motor_state,(float)Motor_Vars.Motor_ActuralDuty,(float)actual_postion,(float)Motor_Vars.motorStateFLAG);

	sprintf(usart3_TxBuffer, "acc: %ld,aim %.2f, act: %ld, state: %.2f, duty: %.2f, theta: %.2f, dir: %.2f\n",
				(int32_t)accelerate_count,(float)motor_state,(uint32_t)Motor_Vars.Motor_ActuralSpeed,(float)motor_state,(float)Motor_Vars.Motor_ActuralDuty,(float)actual_postion,(float)Motor_Vars.Motor_DIR);
	// Position_PI.Umax
//	sprintf(usart3_TxBuffer, "%.2f,%.2f,%.2f\n",(float)actual_postion,(float)Theta_Accumulate,(float)Motor_Vars.Motor_ActuralSpeed);
	HAL_UART_Transmit_DMA(&huart3, (uint8_t*)usart3_TxBuffer, strlen(usart3_TxBuffer));
}

void UART_Print_ViewCount(void)
{
	sprintf(usart3_TxBuffer1, "%.2f,%.2f,%.2f\n",(float)view_count,(float)Motor_Vars.Motor_ActuralSpeed,(float)state_mark);
	HAL_UART_Transmit(&huart3, (uint8_t*)usart3_TxBuffer1, strlen(usart3_TxBuffer1), HAL_MAX_DELAY);
}


void UART_Print_HallState(void)
{
	sprintf(usart3_TxBuffer1, "%.2f,%.2f\n",(float)Hall_Get_State(),(float)Motor_Vars.Motor_DIR);
	HAL_UART_Transmit(&huart3, (uint8_t*)usart3_TxBuffer1, strlen(usart3_TxBuffer1), HAL_MAX_DELAY);
}


void Motor_Start_Task(void)
{
	Motor_Vars.Motor_ActuralDuty = 250;
	__HAL_TIM_ENABLE_IT(&htim1, TIM_IT_COM);
	HAL_TIMEx_HallSensor_Start(&htim2);					  	//开启接口定时器

	// 再手动触发 COM 中断两次，开始正常换向
	soft_trigger_flag = true;
	HAL_TIM_GenerateEvent(&htim1, TIM_EVENTSOURCE_COM);
//	__HAL_TIM_CLEAR_FLAG(&htim1, TIM_FLAG_COM);         // 清除换相中断标志位

	soft_trigger_flag = true;
	HAL_TIM_GenerateEvent(&htim1, TIM_EVENTSOURCE_COM);
//	Motor_Vars.motorStateFLAG = MOTOR_RUN;

}

void Motor_Stop_Task(void)
{
	StopMotor();
	Motor_Vars.motorStateFLAG = MOTOR_IDLE;
}

void Speed_PI_Task(void) // 50us call
{
    Motor_Vars.Motor_ActuralSpeed = Hall_SpeedMonitor_RPM(TIM1_DMA_Buffer, TIM1_DMA_BufferSize);

    actual_postion = 60.0f * accelerate_count;

    // position loop
//	target_speed0 = PIDCalculate((PID_Typedef *)&Position_PI,Theta_Accumulate,actual_postion);

//    Motor_Vars.Motor_AimDuty = PIDCalculate((PID_Typedef *)&Speed_PI, fabs(target_speed0), Motor_Vars.Motor_ActuralSpeed);


//    Motor_Vars.Motor_AimDuty = PIDCalculate((PID_Typedef *)&Speed_PI, Motor_Vars.Motor_AimSpeed, Motor_Vars.Motor_ActuralSpeed);

    Motor_Vars.Motor_AimDuty = PIDCalculate((PID_Typedef *)&Position_PI,Theta_Accumulate,actual_postion);

	float_t tempValue = Motor_Vars.Motor_AimDuty;

	LowPassFilter(&tempValue, 1.0f);

	Motor_Vars.Motor_AimDuty = tempValue;

//	if(target_speed0<0.0f)
//	{
//		Motor_Vars.Motor_DIR = temp_motor_dir^0x01;
//	}
//	else
//	{
//		Motor_Vars.Motor_DIR = temp_motor_dir;
//	}
}

void Position_Speed_PI_Task(void)
{
//    if (Motor_Vars.Motor_spinFlag != 0)
//    {
//        Motor_Vars.Motor_ActuralSpeed = Hall_SpeedMonitor_RPM(TIM1_DMA_Buffer, TIM1_DMA_BufferSize);
//    }
	Motor_Vars.Motor_ActuralSpeed = Hall_SpeedMonitor_RPM(TIM1_DMA_Buffer, TIM1_DMA_BufferSize);

    actual_postion = 60.0 * accelerate_count;

    Motor_Vars.Motor_AimDuty = PIDCalculate((PID_Typedef *)&Position_PI,Theta_Accumulate,actual_postion);

	float_t tempValue = Motor_Vars.Motor_AimDuty;

	LowPassFilter(&tempValue, 1.0f);

	Motor_Vars.Motor_ActuralDuty = tempValue;

    // position loop
//	float target_speed = PIDCalculate((PID_Typedef *)&Position1_PI,Theta_Accumulate,actual_postion);

	// speed loop
//	float target_current  = PIDCalculate((PID_Typedef *)&Speed1_PI, fabs(target_speed), Motor_Vars.Motor_ActuralSpeed);
//	Motor_Vars.Motor_AimDuty  = PIDCalculate((PID_Typedef *)&Speed1_PI, fabs(target_speed), Motor_Vars.Motor_ActuralSpeed);

	// current loop
//	float bus_current = My_adcData[0]/4096.0f*3.3f/200.0f*1e3;
//	Motor_Vars.Motor_AimDuty = PIDCalculate((PID_Typedef *)&Speed1_PI, fabs(target_current), bus_current);


//	float_t tempValue = Motor_Vars.Motor_AimDuty;
//	LowPassFilter(&tempValue, 0.08379f);
//	LowPassFilter(&tempValue, 0.5f);
//	Motor_Vars.Motor_ActuralDuty = tempValue;
//
//	if(target_speed<0.0f)
//	{
//		Motor_Vars.Motor_DIR = temp_motor_dir^0x01;
//	}
//	else
//	{
//		Motor_Vars.Motor_DIR = temp_motor_dir;
//	}

//	actual_postion += Motor_Vars.Motor_ActuralSpeed/60.0f*360.0f*motor_PolePair*50.0f*1e-6;


//	if((Theta_Accumulate-actual_postion)<=0.0f)
//	{
//		actual_postion -= Motor_Vars.Motor_ActuralSpeed/60.0f*360.0f*motor_PolePair*50.0f*1e-6;
//		Motor_Vars.Motor_DIR=(tem_motor_dir^0x01);
//		HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);
//
//	}
//	else
//	{
//		actual_postion += Motor_Vars.Motor_ActuralSpeed/60.0f*360.0f*motor_PolePair*50.0f*1e-6;
//		Motor_Vars.Motor_DIR = tem_motor_dir;
//	}

}


void SpeedControl_Task(void)
{
//	if(Motor_Vars.Motor_ActuralDuty < Motor_Vars.Motor_AimDuty)
//	{
//		Motor_Vars.Motor_ActuralDuty++;
//	}
	if(!Motor_Vars.Motor_FastStopFlag)
	{
		Motor_Vars.Motor_ActuralDuty = Motor_Vars.Motor_AimDuty;
	}
	
}

void SpeedMonitor_Task(void)
{
	printf("%f\r\n", Hall_SpeedMonitor_RPM(TIM1_DMA_Buffer, 6));
	
}

void ModBus_Task(void)
{
	if(Modbus.MessageFlag == 1)
	{
		Modbus_processing();
		Modbus.MessageFlag = 0;
	}
}
	
void UpdateSystem_Log_Task(void)
{
	uint32_t temp = 0;
	
	Modbus.ModBus_ReadReg[0] = system_time_s >> 16;
	Modbus.ModBus_ReadReg[1] = system_time_s & 0xFFFF;
	
	
	float_t temprature = ReadTemprature_TMP112A();
	if(temprature > 0)							//大于零度
	{
		Modbus.ModBus_ReadReg[2] = 10 * temprature;
	}
	else{
		Modbus.ModBus_ReadReg[2] = (10 * (-temprature)) + 0x8000;
	}
	
	Modbus.ModBus_ReadReg[3] = Motor_Vars.ControlMode;
	Modbus.ModBus_ReadReg[4] = Motor_Vars.motorStateFLAG;
	Modbus.ModBus_ReadReg[5] = Motor_Vars.Motor_DIR;
	
	temp = Motor_Vars.Motor_AimSpeed;
	Modbus.ModBus_ReadReg[6] = temp >> 16;
	Modbus.ModBus_ReadReg[7] = temp & 0xFFFF;
	
	temp= Motor_Vars.Motor_ActuralSpeed;
	Modbus.ModBus_ReadReg[8] = temp >> 16;
	Modbus.ModBus_ReadReg[9] = temp & 0xFFFF;
	
	Modbus.ModBus_ReadReg[10] = 0;
	Modbus.ModBus_ReadReg[11] = Foot_Vars.foot_dir;
	
	temp = Foot_Vars.foot_speed;
	Modbus.ModBus_ReadReg[12] = temp >> 16;
	Modbus.ModBus_ReadReg[13] = temp & 0xFFFF;
	
	Modbus.ModBus_ReadReg[14]	= Hand_Vars.hand_dir;
	
	temp = Hand_Vars.hand_speed;
	Modbus.ModBus_ReadReg[15]	= temp >> 16;
	Modbus.ModBus_ReadReg[16] = temp & 0xFFFF;
	Modbus.ModBus_ReadReg[17] = Hand_Vars.hand_temprature;

}

void LED_Task(void)
{
//	HAL_ADC_Start(&hadc1);     //启动ADC转换
//	HAL_ADC_PollForConversion(&hadc1, 50);   //等待转换完成，50为最大等待时间，单位为ms

//	if(HAL_IS_BIT_SET(HAL_ADC_GetState(&hadc1), HAL_ADC_STATE_REG_EOC))
//	{
//		CurrentADC_Value = HAL_ADC_GetValue(&hadc1);   //获取AD值
//		printf("%d\r\n", CurrentADC_Value);
//	}
//	RS485_SendOneByte((uint8_t *)usart1_recvBuff);
//	RS485_SendsomeByte((uint8_t *)usart1_recvBuff, 10);
	HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
	modbusdebug = HAL_GPIO_ReadPin(BREAK_PVDD_GPIO_Port, BREAK_PVDD_Pin);
}

void MotorStateDetect(void)
{
	static uint8_t LastHall = 0;
	static uint8_t cnt = 0;
	
	uint8_t NewHall = Hall_Get_State();
	if(NewHall == LastHall)
	{
		cnt++;
	}
	else{
		cnt = 0;
	}
	if(cnt > 12) // orignal cnt = 12
	{
		Motor_Vars.Motor_spinFlag = 0;
	}
	else{
		Motor_Vars.Motor_spinFlag = 1;
	}

	LastHall = NewHall;
}
